#include <ros/ros.h>
#include "plan_env/grid_map.h"
#include "path_searching/dyn_a_star.h"
#include<Eigen/Geometry>


GridMap::Ptr   grid_map_test_ ; 

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gridmap_test_node");
    ros::NodeHandle nh_("~");

    grid_map_test_.reset(new GridMap) ;
    grid_map_test_->initMap(nh_) ;

    //grid_map_test_->publishMap(); 
    
    AStar::Ptr a_star_;

    Eigen::Vector3d start(0.0, 0.0, 0.0);
    Eigen::Vector3d end(5.0, 5.0, 1.0);

    vector<vector<Eigen::Vector3d>> a_star_pathes;

    a_star_.reset(new AStar);
    a_star_->initGridMap(grid_map_test_, Eigen::Vector3i(100, 100, 100));

    
    if(a_star_->AstarSearch(0.1, start, end))
    {
       //getPath函数的返回值类型是vector<Vector3d>，说明返回的就是一条路径
       a_star_pathes.push_back(a_star_->getPath());       
    }

    for (const auto &vec : a_star_pathes) {
        for (const auto &v : vec) {
            std::cout << v.transpose() << std::endl;
        }
    }
   

    ros::spin();
  
    return 0;
}

